package MelbourneMap;

import java.awt.Shape;
import java.awt.geom.Ellipse2D;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

/**
 * Vehicle is an object that can traverse a {@link Graph}.
 * Each vehicle runs on its own thread and traverses the path
 * between two {@link Node} objects within a Graph.
 * <p>
 * A Vehicle object is created by specifying the Graph, a
 * start Node, a target Node and the maximum amount of steps
 * that the vehicle should search for a path.
 * Running on its own thread a vehicle object searches for
 * and then traverses a path within the specified graph. A
 * shape is provided for drawing the vehicle to the screen.
 * 
 * @author David Zappia, 7183216
 * @author Heong Jin Yu, 7019777
 * 
 */
public class Vehicle extends NotifyingThread
{
	private Graph graph;
	private Node start, target;
	private int limit = 0;
	private ArrayList<Node> path = new ArrayList<Node>();
	private boolean traverse;
	private double xPos, yPos;
	private Shape shape;
	private final int diameter = 200;
	
	/**
	 * Class constructor specifying the {@link Graph}, start and target
	 * {@link Node} objects and the maximum steps to be used when path
	 * searching.
	 * 
	 * @param nodeGraph		the Graph to be searched
	 * @param startNode		the Node to start at
	 * @param targetNode	the Node to be found
	 * @param stepLimit		the maximum allowable search steps
	 */
	public Vehicle(Graph nodeGraph, Node startNode, Node targetNode, int stepLimit, boolean traverse)
	{
		graph = nodeGraph;
		start = startNode;
		target = targetNode;
		limit = stepLimit;
		this.traverse = traverse;
		shape = new Ellipse2D.Double(xPos-(diameter/2), yPos-(diameter/2), diameter, diameter);
	}
	
	@Override
	public void doRun() 
	{
		planPath();

        long startTime = System.currentTimeMillis();
        if (traverse && path != null)
        	traversePath();
        long endTime = System.currentTimeMillis();
		System.out.println("Done! " + (endTime - startTime));
	}
	
	/**
	 * Uses a search algorithm to create a path from the
	 * start Node to the target Node. Path is null if a path
	 * was not found.
	 */
	private void planPath()
	{
		path = dijkstra();
		System.out.println(path);
		//path = AStar();
		System.out.println(path);
	}
	
	/**
	 * Moves the vehicle along the path by using
	 * basic linear mathematics to increment the
	 * x and y positions of the vehicle on the map.
	 */
	private void traversePath()
	{
		xPos = path.get(0).getX();
		yPos = path.get(0).getY();
		
		for (int i = 1; i < path.size(); i++)
		{
			double dx = path.get(i).getX() - xPos;
			double dy = path.get(i).getY() - yPos;
			double D = Math.sqrt((dx*dx) + (dy*dy));
			double xInc = (dx/D);
			double yInc = (dy/D);
			
			while (Math.round(xPos) != path.get(i).getX())
			{
				xPos += xInc;
				yPos += yInc;
				shape = new Ellipse2D.Double(xPos-(diameter/2), yPos-(diameter/2), diameter, diameter);
				//System.out.println("Heading to wayPoint No:" + i + " " + path.get(i).getX() + ", " + path.get(i).getY() +  " Vehicle at:" + xPos +", " + yPos);
				
				try {
					Thread.sleep(5);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			
		}
	}
	
	/**
	 * Turns the route generated by the search algorithm 
	 * into a path made of a list of nodes in the correct order.
	 * 
	 * @param route		the route generated by a search algorithm
	 * @return			the path from the start node to the target node
	 */
	private ArrayList<Node> path(HashMap<Integer, Integer> route)
	{
		if (route.containsKey(target.getID()))
		{
			ArrayList<Node> path = new ArrayList<Node>();
			int currentID = target.getID();
			
			while (currentID != route.get(currentID))
			{
				path.add(graph.getNode(currentID));
				currentID = route.get(currentID);
			}			
			path.add(graph.getNode(currentID));
			Collections.reverse(path);
			return path;
		}
		return null;
	}
	
	/**
	 * Dijkstra Shortest Path First search algorithm.
	 * The Dijkstra algorithm searches through each
	 * node finding the shortest path until a path is 
	 * found between the start node and the target node. 
	 * To aid in search a {@link PriorityQueue} is used.
	 * <p>
	 * It guarantees to find the shortest path possible
	 * based on cost between the vehicle start Node
	 * and target Node if a path exists. The path generated
	 * is in reverse order and must be re-ordered before traversing.
	 * 
	 * @return		the reverse route between the start node and target node
	 */
	private ArrayList<Node> dijkstra()
	{
		Set<Integer> closed = new HashSet<Integer>();
		HashMap<Integer, Integer> route = new HashMap<Integer, Integer>();
		PriorityQueue<Node> open = new PriorityQueue<Node>();
		
		int steps = 0;
		Node leaf;
		Set<Integer> neighbours;
		double cost, cost_f;
		
		start.setPathCost(0.0);
		open.insert(start);
		route.put(start.getID(), start.getID());
		
		while (!open.isEmpty())
		{
			steps++;
			leaf = open.deleteMin();
			cost = leaf.getPathCost();
			closed.add(leaf.getID());
			if (leaf.equals(target))
				break;	
			else
			{
				neighbours = graph.getNeighbours(leaf.getID());
				for (Integer dest : neighbours)
				{
					if (!closed.contains(dest))
					{
						cost_f = cost + graph.getEdge(leaf.getID(), dest).getCost();
						Node n = graph.getNode(dest);
						
						if (open.contains(n))
						{
							if (n.getPathCost() <= cost_f)
								continue;
							else
								open.delete(n);
						}
						
						n.setPathCost(cost_f);
						route.put(dest, leaf.getID());
						open.insert(n);
					}
				}
			}
			if (limit > 0 && steps >= limit)
				break;
		}
		return path(route);
	}
	
	/**
	 * A* Best Path First search algorithm.
	 * The A* algorithm is an informed search that
	 * uses heuristics to find a path between the 
	 * start node and the target node.
	 * <p>
	 * It uses both the cost so far and the estimated
	 * cost, based on distance, between the current node
	 * and the target node to determine which node is
	 * the next best promising to lead to a path.
	 * A* will always find a path with checking the minimum
	 * amount of nodes. The path may not be the shortest but
	 * it can be considered close enough in most cases.
	 * 
	 * @return		the reverse route between the start node and target node
	 */
	private ArrayList<Node> AStar()
	{
		Set<Integer> closed = new HashSet<Integer>();
		HashMap<Integer, Integer> route = new HashMap<Integer, Integer>();
		PriorityQueue<Node> open = new PriorityQueue<Node>();
		
		int steps = 0;
		Node leaf;
		Set<Integer> neighbours;
		double cost, cost_f;
		
		start.setPathCost(0.0);
		open.insert(start);
		route.put(start.getID(), start.getID());
		
		while (!open.isEmpty())
		{
			steps ++;
			leaf = open.deleteMin();
			cost_f = leaf.getPathCost();
			closed.add(leaf.getID());
			
			if (leaf.equals(target))
				break;
			else
			{
				cost = cost_f - graph.cost_h(leaf, target);
				
				neighbours = graph.getNeighbours(leaf.getID());
				for (Integer dest : neighbours)
				{
					if (!closed.contains(dest))
					{
						Node n = graph.getNode(dest);
						double cost_g = cost + graph.getEdge(leaf.getID(), dest).getCost();
						double cost_h = graph.cost_h(n, target);
						cost_f = cost_g + cost_h;
						
						if (open.contains(n))
						{
							if (n.getPathCost() <= cost_f)
								continue;
							else
								open.delete(n);
						}
						
						n.setPathCost(cost_f);
						route.put(dest, leaf.getID());
						open.insert(n);
					}
				}
			}
			if (limit > 0 && steps >= limit)
				break;
		}
		return path(route);
	}

	public Shape getShape() 
	{
		return shape;
	}
}
